#include "FrequencyModeSolver.h"
#include "Utility/Logging/SGLogger.h"
#include "Utility/Algebra/Factory.h"
#include "Utility/Algebra/LinearSolver.h"
#include "Utility/Algebra/sgArpack.h"

#include "DataStructure/Input/Solver/Subcase.h"

#include "DBManager/IService/Input/Dynamic/IEIGRLService.h"
#include "DBManager/IService/Input/Parameter/IParameterService.h"
#include "DBManager/IService/Input/Subcase/ISubcaseService.h"

#include "SGFem/Task/Kernel/MModel.h"

using namespace SG::Algebra;
using SG::DataStructure::Common::Status;
using SG::DataStructure::Common::Real;
using SG::DataStructure::FEM::Project;
using namespace SG::DBManager;

void FrequencyModeSolver::Solve (const DBServiceFactorySP dbServiceFactory, const Project& project, int subcaseId)
{
    // 获取工况参数
    auto                            para = dbServiceFactory->get<ISubcaseService> ()->FindSubCase (subcaseId);
    SG::DataStructure::FEM::Subcase subcase (para);
    Kernel::MModel           model (SG::DataStructure::FEM::SOLType::SOL_SEDFREQ, dbServiceFactory);    
    
    // 频率（起始频率,dw,频率步数）
    std::vector<Real> w;
    if(Status::STATUS_Failure == model.QueryFrenqencePoint (subcase.GetFrequencyId (), _OUT w))
    {
        return;
    }
    
    SG::DataStructure::FEM::EIGRLData EIGRLData;
    if (Status::STATUS_Failure == dbServiceFactory->get<IEIGRLService> ()->FindBySId (subcase.GetMethodId (), EIGRLData))
    {
        return;
    }
    
    
    model.Initialize ({subcaseId});
    model.CalcMatrixEquation (subcaseId);
    
    // 动刚度矩阵
    ComplexSpMat_t KDyna;
    model.ComputeDynaStiffness (_OUT KDyna);
    
    // 质量阵M、粘滞阻尼阵C
    SpMat_t    M, C;                                             
    // model.ComputeStiffnessMatrix (K);
    model.ComputeMassMatrix (M);     
    model.ComputeDampingMatrix (C);
    std::size_t n = KDyna.rows();
  
    // 计算模态
    const int    nModes = EIGRLData.m_ND;
    Vec_t          eigen (nModes);
    Mat_t          phi (n, nModes);
    model.ComputeEigenValue (nModes, eigen, phi);

    const auto pAdj = phi.adjoint ();

    ComplexMat_t   KMode   = pAdj * (KDyna) * phi;
    ComplexMat_t   MMode   = pAdj * M * phi;
    ComplexMat_t   CMode   = pAdj * C * phi;

    // 计算模态阻尼阵
    ComplexSpMat_t C_modaldamp (nModes, nModes);
    for (int i = 0; i < nModes; ++i)
    {
        // 计算 阻尼比
        auto f = sqrt (fabs(eigen[i])) / (2 * M_PI);
        auto Xi = model.ComputeDampingRatio (f);

        C_modaldamp.insert (i, i) = 2.0 * sqrt (fabs(eigen[i])) * Xi;
    }
    auto CTotal = CMode + C_modaldamp;

    // 计算静荷载向量
    model.ComputeFValue (subcaseId);  

    ComplexSpMat_t K_hat; // 动刚度阵 K_hat = -θ² * MMode + i * θ * CTotal + KMode
    ComplexVec_t          P_k;   // theta[k]圆频率下的外力
    ComplexVec_t          P_hat; // theta[k]圆频率下模态坐标系外力
    Vec_t          uMode;
    Vec_t          vMode;
    ComplexVec_t   displacement;
    
    std::string solver_type = "TPardiso<Complex_t>";
    auto        solver      = reinterpret_cast<TLinearSolver<Complex_t>*> (SG::Base::Factory::GetInstance ()->Produce (solver_type));

    for (size_t i = 0; i < w.size(); ++i)
    {
        // 圆频率
        const auto theta = 2.0 * M_PI * w[i];

        // 求解K_hat * displacement = P_hat
        K_hat = -std::pow (theta, 2) * MMode + Complex_t (0.0, theta) * CTotal + KMode;
        solver->compute (K_hat);

        model.ComputeFreqForce (w[i], uMode, vMode, _OUT P_k);
        P_hat = pAdj * P_k;

        solver->solve (P_hat, displacement);

        // 物理坐标下的位移向量
        displacement = phi * displacement;
        model.SetDisplacement (displacement, subcaseId, w[i]);
    }

    delete solver;

    SGInfo << "FrequencyAnalysis Solve fini  \n";
}